#include "PrmPlanner.h"
#include <CGAL/minkowski_sum_2.h>
#include <CGAL/point_generators_2.h>
#include "Kd_tree_d.h"
#include <boost/make_shared.hpp>
#include <CGAL/Boolean_set_operations_2.h>
#include <boost/foreach.hpp>

using namespace std;

PrmPlanner::PrmPlanner(Room room, vector<Polygon_2> robot_polygons, Obstacles obstacles, int robot_num, vector<Conf> start_confs, vector<Conf> target_confs) :
    m_room(room), m_robot_polygons(robot_polygons), m_obstacles(obstacles), m_robot_num(robot_num), m_start_confs(start_confs), m_target_confs (target_confs)
{
    //insert first the start and target points
    m_vertices.push_back(m_start_confs);
    m_vertices.push_back(m_target_confs);
    //initialize the Graph and kd-Tree
    m_graph = new Graph<LABEL>(0, true);
    m_kd_tree = new Kd_tree_d<Kernel_d>();

    BOOST_FOREACH(Polygon_2& robot, m_robot_polygons)
    {
        CollisionDetector* collision = new CollisionDetector(robot, &m_obstacles);
        Sampler* sampler = new Sampler(robot, m_room, collision);
        m_cols.push_back(collision);
        m_samplers.push_back(sampler);
        vector<CollisionDetector*> cols;
        BOOST_FOREACH(Obstacles& other, m_robotObs)
        {
            CollisionDetector* collision2 = new CollisionDetector(robot, &other);
            cols.push_back(collision2);
        }
        m_colsRobots.push_back(cols);
        Obstacles next;
        next.push_back(robot);
        m_robotObs.push_back(next);
    }
}

PrmPlanner::~PrmPlanner()
{
    for (int index = 0; index < m_robot_num; ++index)
    {
        delete m_cols[index];
        delete m_samplers[index];
        for (int j = 0; j < m_colsRobots[index].size(); ++j)
        {
            delete m_colsRobots[index][j];
        }
    }
    m_cols.clear();
    m_samplers.clear();
}

/*	This function is invoked by the GUI and is assumed to update the resulting */
void PrmPlanner::run()
{
    generate_roadmap();

    update_path();

    //	run this method when you finish to produce the path
    //	IMPORTANT: the result should be put in m_path
    transform_path_for_gui(m_robot_num, m_path, m_scene);
}


void PrmPlanner::update_path()
{
    list<LABEL> path;                   // The points indexes
    m_graph->find_path(START_LABEL, END_LABEL, path);

    list<LABEL>::const_iterator iter;
    //for each point-index add the point to the generated_path
    for(iter = path.begin(); iter != path.end(); ++iter)
    {
        m_path.insert(m_path.end(), m_vertices[*iter]);
    }
}

//	This operation is supposed to generate the roadmap (sample configurations
//	and connect them.
// number_vertices: number of sampled vertices
// k_nearest:maximal degree of vertex in roadmap
void PrmPlanner::generate_roadmap(int number_vertices, int	k_nearest, double eps)
{
    int last_index = 0;
    //Add m_number_vertices to the vector
    while (last_index < number_vertices)
    {
        vector<Conf> p = generate_sample();
        ++last_index;
        m_vertices.push_back(p);
    }
    //add all points to the graph as vertices
    int index = 0;
    for (index = 0; index < m_vertices.size(); ++index)
    {
        m_graph->add_vertex(index);
    }
    index = 0;
    //insert all points to the kd-tree
    BOOST_FOREACH(vector<Conf>& points, m_vertices) {
        std::vector< double > coord;

        BOOST_FOREACH(Conf& point, points){
            coord.push_back(point.x().exact().to_double());
            coord.push_back(point.y().exact().to_double());
        }
        Point_d point_d(m_robot_num * 2, coord.begin(), coord.end());
        m_kd_tree->insert(point_d);
        //add each point to the maps
        m_expandMap[points] = point_d;
        m_map[point_d] = index;
        ++index;
    }

    // go over all the points
    BOOST_FOREACH(vector<Conf>& p, m_vertices){
        vector<Point_d> nearest_neighbors;
        // find the k- nearest neighbors
        m_kd_tree->k_nearest_neighbors(m_expandMap[p], k_nearest, std::back_inserter(nearest_neighbors));
        int p_index = m_map[m_expandMap[p]];
        BOOST_FOREACH(Point_d& q, nearest_neighbors){
            int q_index = m_map[q];
            // if there isn't an edge between them (or the connected components) and there is a valid-path (by the local planner) between them
            if (!m_graph->is_in_same_cc(p_index, q_index) && local_planner(p_index, q_index, eps))
            {
                // add an edge to the graph
                m_graph->add_edge(p_index, q_index);
            }
        }
    }
}

vector<Conf> PrmPlanner::generate_sample()
{
    {
        while (true)
        {
            vector<Conf> sample;
            //generate sample for each robot
            for (int index = 0; index < m_robot_num; ++index)
            {
                sample.push_back(m_samplers[index]->generate_sample());
            }

            //make sure they don't overlap
            
            bool is_colliding = false;
            for(int index = 0; index < m_robot_num; ++index)
            {
                for(int j = 0; j < index; ++j)
                {
                    if (!m_colsRobots[index][j]->valid_conf(sample[j]))
                    {
                        is_colliding = true;
                        break;
                    }
                }
                if (is_colliding)
                {
                    break;
                }
            }
            if (!is_colliding)
            {
                return sample;
            }
        }
    }
}

bool PrmPlanner::local_planner(int start_index, int target_index, double eps)
{
    vector<Conf> startConf = m_vertices[start_index];
    vector<Conf> targetConf = m_vertices[target_index];

    if (is_colliding(start_index, target_index, eps))
        return false;

    //use collision detector to find yes/no for each robot
    for (int index = 0; index < m_robot_num; ++index)
    {
        if (!m_cols[index]->local_planner(startConf[index], targetConf[index], eps))
            return false;
    }
    return true;
}

bool PrmPlanner::is_colliding(int start_index, int target_index, double eps)
{
    vector<Conf> startConf = m_vertices[start_index];
    vector<Conf> targetConf = m_vertices[target_index];
    
    for(int index = 0; index < m_robot_num; ++index)
    {
        for(int j = 0; j < index; ++j)
        {
            Vector_2 vecS = startConf[j] - startConf[index];
            Vector_2 vecE = targetConf[j] - targetConf[index];
            Point_2 s(vecS.x(), vecS.y());
            Point_2 e(vecE.x(), vecE.y());
            if (!m_colsRobots[index][j]->local_planner(s, e, eps))
                return true;
        }
    }

    return false;
}
